SVERIGE 
09) SE 




(12) UTLAGGNINGSSKRIFT IB] (21) Q4Q424e _ 4 

(51) Internationel! klass 4 B253 9/10 



(44) Ansokan utiagd och utlagg- 
nmgssknften pubhcerad 

(41) Ansokan alimant tMlganghg 

(22) Patentansokan inkom 

(24) Lopdag 

PATENTVERKET (62) Stamansokans nummer 

(66) Internationell mgivmngsdag 

(86) Ingivningsdag for ansokan 
om europetskt patent 

(30) Pnontetsuppgifter 



86-04-2 1 
86-02-28 
84-08-27 
84-08-27 



(1 1) Pubhcenngs- 
nummer 



444 530 



Ansokan inkommen som; 

S3 svensk patentansokan 

□ fullfoljd internationell patentansokan 
med nummer 

□ omvandlad europeisk patentansokan 
med nummer 



(71) Sokande AS EA AB, 72 1 83 Vasteras SE 

(72) Uppfinnare 3 G* Fager , VasterSs 
(74) Ombud Bhman 8 

(54) Benamning Industrirobot forsedd med lagesgivaranordning 

(56) AnfSrda Publikationer : EP 114 505 (B25J 19/00) DE 3 228 373 (G05B 19/19) 
US 4 453 085 (250-203R) 

(57) Sammandrag: 

En industrirobot (2) har ett flertal relativt varandra rdrliga delar (21-25). 
Roboten ar utrustad med en lagesgivaranordning som har en givare (30) som 
a> monterad pa robothanden (27)- Givaren mottar signaler fran en uppsattning 
(Zl1_t|ll) av minst tre ljuskallor med kanda lMgen . Givaren avkanner rikt- • 
ningarna fran givaren till de olika 1 juskallorna , och ur dessa riktningar 
samt ljuskallornas kanda lagen beraknas robothandens lage och eventuellt 
orientering relativt 1 juskSllorna av i utrustningen anordnade berSknings- 
organ . 
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Fdreliggande uppfinning avser en industrirobot med ett flertal relatlvt var- 
andra rdrliga delar. 

Vid en industrirobot vill man av flera orsaker kunna bestamma robotens lage 
relativt ett k§nt koordinatsystem. Den kanske viktigaste av dessa orsaker ar 

5 att man under drift onskar erhalla en lagesinforraation son kan aterkopplas 
till robotens styrsystenu Det hi tt ills normalt anvanda sa'ttet att erhalla 
den onskade lagesinformationen a*r att anordna givare, t ex vinkelgivare ? f6r 
var och en av robotens fr ihetsgrader och att med ledning av givarsignalcrna 
. och de kanda dimensionerna hos robotens delar berSkna robotens lage relativt 

10 robotens fastsattningspunkt . Detta satt har dock flera nackdelar . Med ut- 

trycket "robotens lage" avses hSr laget hos en viss del ov roboten, vanligen 
robotens hand eller ett av roboten buret verktyg. 

Fbr att fa god noggrannhet hos lagesinformationen krfivs hog noggrannhet hos 
givarna och hog mattnoggrannhet och sma glapp hos robotens mekaniska delar. 
15 Vidare krSvs att roboten ar sa kraftigt byggd att dess deformation under be- 

lastning blir liten. Dessa krav gor att roboten blir relativt tung och klumpig 
och far ett h6gt tillverkningspris - 

For att fa korrekt lagcsinformation relativt ett fast koordinatsystem kravs 
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vidare att robotens fastsattning (t ex raonteringen av robotens fot i ett 
golv) gdrs med hog noggrannhet. 

Qn inte tillverknings- och monteringstoleranser hills raycket sma uppstar 
ocksa problem med utbytbarheten, dvs det blir inte utan vidare roSjligt att 
utan oraprogrammering utbyta ett robotexemplar mot ett annat. 

Det blir vidare svart att med bibehallen noggrannhet^erhalla ett stdrre 
arbetsomrade for roboten an det som erhalles na*r roboten star fast monterad. 

Uppfinningen avser att astadkomma en industrirobot med en lagesgivaranord- 
ning som gor det mojligt att erhalla hog noggrannhet hos ISgesinforraationen 
oberoende av tillverknings- och fastsa'ttningstoleranser och oberoende av de- 
formationer hos robotens delar. Uppfinningen avser vidare att astadkomma en 
industrirobot, d3r robotexemplar utan omprogrammering kan ufcbytas mot var- 
andra utan att nigra krav stalls pi sma tillverknings- och fastsSttningstole 
ranser. Ett ytterligare syfte med uppfinningen ar att astadkomma en industri 
robot, vars lagesin format ion Sr oberoende av att laget hos robotens fastsatt 
ningspunkt Sr kfint, varigenom roboten kan tillatas r5ra sig godtyckligt inom 
en lokal och dess arbetsomrade kan goras mycket storre an vad som hittills 
varit mojligt. 

Uppfinningen avser vidare att Astadkomma en enkel och robust givaranordning 
Tor alstrande av den Snskade ISgesinformationen. 

Vad som kannetecknar en robot enligt uppfinningen framgar av bifogade patent 
krav . 

Uppfinningen skall i det foljande narmare beskrivas i anslutning till bifo- 
gade figurerl-7. Fig 1 visar un industrirobot med ett exempel pi en la*gef3- 
givarutrustning enligt uppfinningen. Fig 2a visar principen fdr den pi 
roboten i fig 1 monterade givaren. Fig 2b visar den i givaren ingiende detek 
torn. Fig 3 visar ett blockschema over lagesgivaranordningen och fig *\ visar 
hur den 3r inkopplad till robotens styrsystem. Fig 5 visar vissa av de an- 
vSnda koordinaterna och fig 6 de anvanda vektorbeteckningarna f6r Ijuskal- 
lornas och givarens lagen. Fig 7 visar ett flodesschema for de berakningar 
som ur givarsignalerna ger robotens lage. 
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Fig 1 visar en industrirobot 2, som ar fast monterad pa ett underlag 1, t ex 
ett golv. Roboten a> av i och Tor sig kSnd typ och har en fot 21. En pelare 
22 a*r vridbar runt en vertikal axel i fdrhallande till foten 21 . En under- 
arm 23 £r vridbart lagrad till pelaren 22 med hjSlp av en horisontell axel. 

5 En Sverarm Zh Sr lagrad till underarmen 23 och en robothand 25 till 5ver- 
armen 2*J, bada roed hjalp av i figuren horisontella vridni ngsaxlar . Vid 
robothanden 25 ar en roellandel 27 fSst , vilken har ett verktygsfa*3te 28, 
som uppbSr ett gripdon 29- Pa mellandelen 27 Sr en optisk givare 30 anord- 
nad, vilken pa nedan beskrivet satt anvands for att besta*mma robothandens 

10 lage och orientering i rymden. Roboten har ett styrsystem 5, vilket mojlig- 
gor in programme ring av ett rorelseschema hos roboten med 5tf51jande drift 
av roboten i enlighet med detta rorelseschema. Styrsystemet ha*mtar en 
successiv foljd av lagesbSrvSrden ur programmet, Jamfdr dessa med avkanda 
arvarden for robothandens lage och driver motorerna i robotens olika axlar 

15 sa att robothanden f61Jer de inprogrammerade ISgesbdrvSrdena. For be s tarn - 
ning av robothandens lage och samverkan med givaren 30 a> en uppsattning 
ljuskallor -4 1 — -U^* anordnade. Ljuskallorna kan larapligen utgSras av lysdioder 
och ar sa anordnade att under drift av roboten minst tre och heist fyra av 
ljuskallorna hela tiden bef inner sig inom givarens 30 synfait. Pa nedan 
20 nSrmare beskrivet satt bestSmmer givaren 30 riktningarna ho3 syftlinjerna 
fran givaren till 1 juskSlloma . LjuskSllornas lagen i ett visst koordinat- 
system forutsStts kSnda och matas larapligen in i styr3y3teraet vid pro- 
grammering eller installation av roboten. Ur dessa pa fdrhand kanda l£gen 
och de av givaren 30 bestamda riktningarna beraknar styrsystemet pi nedan 
25 beskrivet sStt givarens lage i det aktuella koordinatsystemet . Ljuskallorna 
kan exempelvis vara fast anbringade, och robotens lage berMknas da i ett 
fast koordinatsystem. Alternativt kan ljuskallorna anbringas vid ett 
arbetsstycke som roboten skall bearbeta eller hantera, och robotens 12ge 
erhalles da i forhallande till arbetsstycket . 

30 PS i och for sig kant satt Sr roboten forsedd med vinkelgivare i sina axlar, 
vilka till styrsystemet avger information om de aktuella vridningsvinklarna 
i axlarna. Denna information anvands av styrsystemet for att fbrdela ett upp~ 
tradande lagesfel pa de olika robotaxlarna . Dessa givare kan utforas roed va- 
sentligt lagre noggrannhet Sn vad som varit nodvandigt vid tidigare kSnda 

35 robotar, dar givarna anvSnds fdr bestamning av robothandens lSge. 

Av naturliga skSl kan givaren 3Q oftast ej placeras i den punkt, vars lage 
man egentligen vill bestamma. Denna intressanta punkt utgbrs ju t ex av 
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gripklorna hos ett gripdon, av spetsen hos en svetselektrod eller av eggen 
hos ett bearbetningsverktyg. Med kannedora om givarens lSge i rymden och otn 
robothandens orientering kan dock enkelt laget f6r den nSmnda punkten be- 
ra'knas . Information om robothandens orientering kan, som nedan nSrmare 
skall beskrivas, erhallas med hjalp av lagesgivarsystemet enligt uppfin- 
ningen. Eftersom givaren 30 foretradesvis placeras sa nSra den aktuella 
punkten som mojligt, kr&vs ibland ingen storre noggrannhet hos bestSmningen 
av handens orientering, och dSrfSr kan alternativt de i robotaxlarna anord- 
nade vinkelgivarna anvandas for att ge information om handens orientering . 

For att mojliggora en lSges- och orienteringsbestSmning av robothanden obe- 
roende av dennas orientering relativt signalkSllorna kan ytterligare givare 
anordnas pa handen pa sadant 3att f att under robotens drift alltid minst en 
av dessa givare (eller bada tillsammans) har ett tillrSckligt antal ljus- 
kallor inora sitt synfalt. En sadan ytterligare givare ar visad med streckade 
linjer i figuren och Sr betecknad med 31. 

For bestamning av lage och/eller orientering hos ovriga robotdelar kan om 
sa onskas £ven dessa delar forses med lagesgivarsystera enligt uppf inningen . 
Som ett exempel har i figuren visats hur en givare 32 ar anordnad pa under- 
arraen 23. En upps§ttning ljuskallor 45-^8 ar sa anordnad att minst tre och 
heist Tyra av ljuskallorna under robotens drift befinner sig inom givarens 
32 synfalt. Den salunda for underarmen 23 anordnade lagesgivarutrustningen 
kan anvandas for bestamning av underarmens lSge och orientering. Vid en ak- 
bar robot kan fdr bestamning av robotens aktuella lfige inom sitt akomrade 
en dylik givare placeras pa t ex foten 21. 

j 

Alternativt till att montera mer &n en givare pi robothanden kan givarut- 
rustningen forses med fler an de fyra i fig 1 visade 1 juskallorna. Ljus- 
kSllor anordnas da i tillrSckligt antal, t ex sex-atta stycken t och med 
sadana lSgen att, oberoende av robothandens ISge och orientering, alltid 
minst tre eller heist fyra av ljuskallorna befinner sig inom givarens syn- 
falt. Som nedan skall beskrivas anvander givarsystemet for bestamning av 
lage och orientering tre at gangen av 1 juskallorna. Robotcn kanner hela 
tiden till sitt lage och givarens orientering och styrsysteraet kan dSrf6r 
programmeras att i varje lage va*lja ut och for lSgesbestamningen anvanda 
tre inom givarens synfalt lSmpligt placerade IjuskSllor. Spra visas i fig 1 
har styrsysternet en 3ignalforbindelse med ljuskSllorna och Mr anordnat att 
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aktivera de IjuskSllor som for tillfallet ar aktuella for mStning. Den nedan 
beskrivna utforingsformen av givaren 30 kan endast bestamma riktningen till 
en ljuskSIla at gangen , och vid anvSndning av denna typ av givare ar darfor 
styrsystemet anordnat att pulsa de tre eller fyra anvanda ljuskSllorna med 
lamplig frekvens och pa sadant sStt att endast en ljuskalla at gangen ar 
5 tand . 

Av latt insedda skal blir noggrannheten i lagesbestUraningen std>re ju narmarc 
robothandcn ljuskSllorna ar anbringade. Det kan darfOr vara lampligt att ha 
en uppsattning ljuskallor placerade pa sa stort avstand fran roboten att de 
kan anvSndas inom robotens hela arbetsomrade och dSrutover en ytterligare 
10 uppsattning som ar sa placerad sa nara som mojligt intill den eller de 
arbetspunkter da> h8g noggrannhet krSvs. 

Fig 2a visar principert for givaren 30 1 fig 1 . Givaren har en vidvinkellins 
300, som ar symraetrisk runt givarens axel 301 . Linscn kan exempelvis vara 
approximativt halvsfSrisk och ar sa" utformad att den har en sa stor opp- 

15 ningsvinkel som mojligt. Foretradesvis bor oppningsvinkeln vara minst 2* 

steradianer. VinkelrStt mot axeln 301 a> en plan tvadimensionell fotodetek- 
tor 302 anordnad. Linsen avbildar en ljuska'lla at gangen pa detektorn, dvs 
infallande ljus 313 frin en ljuskalla bryts samman i en punkt 314 pi detek- 
torn. Linsens bra*nnvidd kan gdras sa kort att tillrSckligt god fokusering 

20 erha"lles f6r alia aktuella 1 juskiilleavstand. En viss oskarpa i avbildningen 
spelar ocksi mindre roll, eftersom den i fig 2a och 2b visade givaren be- 
stSmmer lSget hos tyngdpunkten hos ljuskallans avbildning pa detektorn. 

Detektorn utgors som visas i fig 2b av en plan platta av kisel med ett cen- 
tralt egenledande skikt, ett p3 Bversidan anordnat P-ledande skikt och ett 
25 pa undersidart anordnat N^-ledande skikt. P-skiktet ar forsett med lang- 
strackta kontakter 305-306 respektive 303-30*1 vid sina motstaendc 3idor. 
N + -skiktet a> forsett med en kontakt 307- Fotostrommens fordelning mellan 
de b3da motstaende kontakterna bestSms av resistansen och darmcd av av- 
standet fran ljusflacken till respektive anslutning. 

30 I fig 2a ar de vid berSkningarna anvanda koordinatsystemen utsatta. Med 

hjalp av kontakterna 305 och 306 samt en forstaVkare 310 och en A/D-omvand- 
lare 312 erhalles en digital signal x, som ar ett matt pa 1 jusf lackens av- 
stand i x-led fran axeln 301. Med hjalp av kontakterna 303 och 304, for- 
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starkaren 309 och A/D-omvandlaren 311 erhalles pa motsvarande sStt en digi- 
tal y, som Sr ett matt pa ljusf ISckens avstand i y-led fran axeln 301. Med 
hjalp av signalerna x och y kan, som nedan narmare skall beskrivas, Ijus- 
f ISckens lage pa detektorytan i polSra koordinater a, p bestSmmas. Ur dessa 
koordinater erhalles sedan de sfSriska koordinater 9. 9 som definierar rikt- 
ningen hos syftlinjen 3*3 till den aktuella ljuskSllan. 

I stallet for den i fig 2 visade tvadimensionella analoga detektorn kan 
andra motsvarande typer av detektorer anvandas. Ett exempel ar en s k CCD 
(charge coupled devise) -detektor, vilken kan utfSras tvadimensionell . Ett 
annat exempel pa en sadan detektor ar en s k fo tod iod array, vilken Sven 
den kan utforas tvadimensionell . En fordel raed en sadan array a'r att man 
inte endast kan avkanna tyngdpunkten hos ljusflacken utan med hjalp av 
laroplig signalbehandling kan bestSmma laget hos t ex den punkt hos ljus- 
flacken, dar ljusstyrkan Sr maximal. 

Som ett alternativ till den i fig 2 visade givaren kan tva endimensionella 
givare anvSndas. Dessa anordnas da med sina aykSnningsriktningar f6retrS- 
desvis vinkelratt mot varandra och med givarna sa nara varandra som mQjligt. 
Varje givare bestar da av en cylinderlins och endimensionell f otodetektor , 
exempelvis en analog detektor eller en endimensionell diodarray. 

Fig 3 visar ett blockschema over lagesgivarutrustningen enligt uppf inningen . 
En styrenhet 50 avger klockpulser och eventuellt for samordning och synkroni- 
sering ytterligare nodvandig information till utrustningens ovriga enheter. 
Denna information Sr i figuren betecknad med cl. Givaren 30 ar utftfrd enligt 
fig 2 och avger signaler x, y till enheterna 51 och 52. Enheten 52 a'r en be- 
rakningsenhet, som med hjSlp av signalerna x, y samt lagrade data ora bl a 
ljuskSllornas ISgen berSknar information' som definierar robothandens lSge X. 
och orientering Z. Berakningsenheten 52 bestSmmer aven genora derivering av 
vektorerna X och Z hastighetsvektorerna X och Z. Dessa fyra vektorer eller 
signaler tillf8rs robotens styrsystem for styrning av robotens rorelse. En 
ljuskallestyrenhet 51 har med var och en av ljuskSllorna 41-48 forbundna 
utgangar (endast Ijuskallan 41 ar visad i figuren). Denna styrenhet mottar 
information X och Z om robothandens lage och orientering samt information 
x, y om lju3f ISckens ISge pa detektorn. I beroende av denna information ut- 
vSljer styrenheten 51 ISraplig uppsattning ljuskallor och aktiverar dessa, 
foretradesvis genom periodisk pulsning med ISmplig frekvens sa att endast en 
av de utvalda ljuska'llorna at gangen Sr lysande. Styrenheten 51 avger infor- 
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raation till berakningsenheten 52, vilken definierar den f5r tillfailet ut- 
valda uppsattningen ljuskallor. 

Enheten 30 kan utforas pa det i fig 2 visade sSttet. Enheten 50 kan utgdras 
av en enkel klockpulsoscillator e-ller av en pa ISmpligt satt programmerad 
5 mikrodator. Enheten 51 kan utgoras av en specialbyggd logisk koppling eller 

av en mikrodator. Berakningsenheten 52, sora skall utfOra de nedan i samband 
med fig 7 beskrivna berakningsoperationerna , kan pa i och for sig kant satt 
besta av digitala eller analoga kretsar eller av en pS lampligt satt pro- 
grammerad mikrodator. 

10 Fig 4 visar hur lagcsgivarutrustningen enligt uppfinningen ar ansluten till 
robotens styrsystem. Civaren 30 avger signalerna x, y till berakningsenheten 
52, vilken i sin tur beraknar en vektor X som definierar robothandens l£ge. 
Denna signal tillfors en komparator 53- Till en andra ingang pi komparatorn 
matas lagesbdrvarden Xq fran robotens programverk 58 i lSmplig takt. Fran 
15 komparatorn erhalles i varje ogonblick en signal AX som definierar lagesav- 
vikelsen hos robothanden. Denna ISgesavvikelse tillfdrs en s k axeluppdelare 
5^, i vilken avvikelsen transformeras till robotens koordinatsystem, dvs 
delas upp pa ett fel i var och en av robotens axlar. Axeluppdelaren 5*1 har 
alltsa en utgang f6r var och en av robotens axlar. I figuren 5r endast ut- 
20 signalen - - for axeln 1 visad. Denna signal tillfors axelna 1 regulator 
56. Fran enheten 52 erhalles fiven en vektor X som definierar den momentana 
hastigheten hos robothanden. I enheten 55 JamfcJrs denna hastighet med ett 
bdrvarde for hastigheten. Skillnaden mellan de bada varden som tillf drs 
enheten 55 uppdelas dSrefter pa robotens olika axlar, dvs transformeras over 
2 5 till robotens koordinatsystem. Enheten 55 har en utgang far var och en av 
robotaxlarna, men endast utsignalen Ar ^ f6r axeln 1 ar visad i figuren. 
Denna signal tillfors axelns 1 regulator 56. Regulatorn 56 styr pa i och 
f6r sig kant satt enheten 57, 3om innehaller ef fektforsta'rkare och driv- 
motor for axeln 1 . Utsignalen fran drivenheten 57 utgors alltsa av en vrid- 
30 ning hos robotens axel 1, och denna vridning - 1 figuren med figuren beteck- 
nad med y^ - paverkar i sin tur laget och ori enteringen hos robothanden och 
givaren 30. Med hja'lp av det i fig 4 visade systemet kommer alltsa roboten 
att styras sa att givaren 30 foljer den nana som bestams av de successivt 
frammatade lagesborvardena fran programenheten 58. 

35 I fig H har fdr enkelhets skull endast lagesbestSmningen och -regleringen 

visats. Reglering av robothandens orientering kan gdras p3 motsvarande satt 
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i beroende av den information om den verkliga ori enteringen Z som erhalles 
fran berakningsenheten 52 och av borvarden for orienteringen, vilka matas 
frara av progranverket 58. 

Fig 5 visar nigra av de beteckningar , sora. anvands vid de berSkningar som 
utf 6rs av berakningsenheten 52. Figuren visar scheraatiskt ytan hos detek- 
torn 302 i fig 2, sedd uppifran. DetekLorns rHtvinkliga koordinatsystem 
x, y ar markerat, liksom de polSra koordinaterna a, p for var och en av de 
tre ljuskallorna a, b, c. 

Fig 6 visar de tre ljuskallorna a, b och c, vilka i ett visst tidsavsnitt 

anvands for lages/orienteringsbestamningen . Vektorerna A, och B 2 definie- 

rar laget hos givaren 30, ljuskSllan b respektive ljuskallan c i forhallande 

till ljuskSllan a. Vektorn Sr vektorprodukten av vektorerna B^ och B^- 

Vektorerna P . P L och P definierar 1 juskallornas a, b och c lage i det 
— a — p — c 

koordinatsystem dar ljuskallorna a> anordnade. Vektorn X betecknar givarens 
30 lage i samma koordinatsystem . 

Bera'kningsenhetens 52 funktion skall nu nSrmare beskrivas i anslutning till 
fig 7- Denna enhet kan utforas med specialbyggda kretsar for 16sning enligt 
kanda principer av de nedan angivna sambanden. Alternativt kan den utgtfras 
av en pa lampligt satt prograramerbar (mikro)dator . For overskadlighets skull 
antas det senare vara fallet, och fig 7 visar ett f lodesschema for beraknings- 
gangen hos en sadan (mikro)dator . 

De tre vid varje berakning anvanda 1 juskSllorna ger upphov till tre par cv 
sammanhorande x-y-varden pa ytan av detektorn 302. Vilka riktningar i ruramet 
som motsvaras av dessa x-y-varden beror delvis pa utforandet av givaroptiken. 
I fortsattningen antas for enkelhets skull att R i fig 2 Sr konstant (obero- 
ende av 6). Detta behover dock ej vara fallet, men berakningarna blir nagot 
rner komplexa om si inte skulle vara fallet. R(6) ar namligen ingen obekant 
storhet utan endast en ytterligare parameter att ta hansyn till vid berak- 
ningarna . 

Vid berakningens borjan erhaller berakningsenheten information fran styr- 
enheten 51 om vilka tre ljuskallor som anvSnds vid berSkningen. Dessa ljus- 
kSllor betecknas med a, b, c. En ljuska'llas M i M avbildning pa detektorytan har 
i dennas koordinatsystem laget x i y .. . Avbi ldningens la*ge i polara koordinater 
ges av foljande ekvationer: 



8404246-4 



10 



a. = arctan & + (, - - f 



-/ 



Ur dessa pola>a koordinater erhalles oedan de srSriska koopdinaterna for 
syftlinjen till ljuska*llan ur ekvationerna : 

*i = a i 
Q i - f « J 1 ) 

Funktionen f(p A ) beror av optlken och kan en gang for alia uppraatas och lag- 
ras i berakningocnhcten. Optiken bor dock vara aa utformad att funktionen 
a> monoton i det aktuella intervallet. Harefter beraknas vinklarna T 1 , 
och mellan de tre syftlinjerna fran givaren till ljuskallorna med hjalp 
av fdljandc ekvationer: 

c i • », - » b 

15 (a, b, c antas ordnade sa att <p > <p > 9 ) 

a D c 



= arcsin 



(sinC^ 



sin G 



0 -e. 



(cot -j-)sin — (cot y-) cos 
arctan - — 5 + arctan Q ~ 

9 e +e 



sin 



a b 



cos 



a b — 1 



t 2 = arcsin 



sin 9 



(sinC 2 ) - 



(cot -|) sin -S— £ (cot 2 } cos ca 
arctan ^ — ^ + arctan 



sin 



e +e 

c a 



cos 



c a 
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sin e b 




arcsin 



(sinC_) 





(cot — ) sin 



(cot -~ ) cos 



arctan 



+ arctan 




Flodesschemats fyra forsta rutor har nu behandlats. I det femte steget bildas 
funktionerna G (A) enligt: 



da> b 1 ar avstandet mellan ljuskallorna a och b 
b^ ar avstandet roellan ljuskallorna a och c 
b^ Sr avstandet mellan ljuskallorna c och b 

Denna funktion ar egentligen atta olika funktioner, en for var och en av de 
10 atta kombinationerna av tecken vid de tre stSllen i uttrycket dar antingen 
ett plus- eller minustecken kan forekomma (tecknet framfdr uttrycket 

Vb| - A 2 sin 2 T 2 har alltid samma varde pa de bada stallen dar uttrycket fore- 

kommerK Dessa funktioner betecknas i f lodesschemats fetnte ruta med (A) , 
dar i = 1 8. For var och en av dessa funktioner bestaromes nollstallen i 

15 de delar av funktionen, dar var och en av funktionens bada termer liksom 

AcosT 2 t Jb* ~ A 2 sin 2 T 2 ar sa vMl reell (betecknas raed Z 1 > 0 i f lodessche- 
mats sjatte ruta) som positiv (betecknas med [ ] > 0 i f 16desschemats 
sjatte ruta). I nasta ruta undersoks hur manga nollstallen som totalt har 
erhallits. Om endast ett nollstSlle har erhallits gar berskningsprogrammet 

20 vidare till den med B, C bctecknade rutan i flodesschemat. 0m raer fin ett 

nollstalle har erhallits valjs i nasta punkt av be rakn ings schema t de noll- 
stallen bort dar A jamfort med foregaende berakningar skulle medfora en 
prealistiskt hQg hastighet hos roboten. Om efter denna kontroll endast ett 
nollstalle aterstar gar berakningsprogrammet vidare till rutan B, C. Om 

25 fortfarande flera nollstallen aterstar och om den aktuella passagen genom 

flodesschemat ar den forsta passagen utvaljs en ny grupp av tre ljuskallor, 
i flodesschemat betecknade med a, b, d. Om sa ar fallet gors den ovan 
beskrivna berakningen om fran borjan for den nya gruppen av ljuskallor. 
Darefter utfores den nedan beskrivna lagesbestamningen for bada uppsatt- 



i 
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ningarna av ljuskallor, varefter den 16sning valjes som ger en gemensam 
position. Ora den aktuella passagen genom berSkningsschemat inte ar den 
rarsta passagen har enligt ovanstiende beskrivning berakning av noli- 
stallen gjorts for tva uppsattningar av ljuskallor, beriikningen gar vidare 
enligt nedanstaende beskrivning och till slut den losning utvSljes som ger 
ett gemensamt lage ror de bada uppsSttningarna av ljuskSllor. • 

Enligt nasta steg i berSkningen, betecknat med B, Ci f lodesschemat , teck- 
nas forst uttrycken; 



2 2 2 

*o + b T - 

„ = arccos 

y = arccos =-r— 

2A 0 b 2 

e - arccos rr , 

2b 1 b 2 

dar A Q ar det va'rde pa A dar funktionen G( A) = 0 och B respektive C gives 
av uttrycken: 

B = A Q cosT 1 - Vb 2 - A^sin 2 ^ 



*5 C = A rt cosT„ - Jb* - A^sin 2 T^ 



da> de dubbeltydiga tecknen i uttrycken for B och C va'ljes som tecknen i mot- 
svarande uttryck i den ekvation G.(A) som gav det aktuella nollsta* llet . Har- 
erter bildas storheterna s och k enligt: 



s = n + 1 * e 



20 |< - / sln(s-ir)gin(s-n)Bln(s-e7 
si n s 



©ami storheterna x och 6 enligt uttrycken: 
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x = 2 arctan 



sin(s-Y) 
6 s arcsin (sinx sinn) 



varvid om x > an \ losningen 6 > | v31 jes och om x < | losningen 6 £ | vai jes 

Som framgar av fig 6 ges riktningen fran lJuskMllan a till b av vektorn g 1 
och fran ljuskailan a till c av vektorn ^ Riktningen fran ljuskallan a till 
givaren 30 betecknas raed enhetsvektorn A. Vektorn utgor vektorprodukten 
av vektorerna B och * z - Vektorn A kan nu losas ur foijande ekvationssystem: 




Givarens 30 lSge, vilket betecknas av vektorn X erhalles harefter ur: 



10 X = A * A + P 
— 0 — —a 



Givarens 30 lage har nu bestamts. 



15 



Harefter be3tams givarens orientering genom att foijande vektorer bildas: 
P - X 



Z = 



£1 
x 



z = 



z+ — 'I 

P - X 

— c — 

|P_ - XI 



Harefter erhalles komponentema n^, n b och n Q ho3 normalvektorn till givaren 
detektoryta 302 ur ekvationssystemet : 



\ 



f z z z 

ax ay az 

Z. Z K 
bx by bz 



Z Z Z 
cx cy cz 



/ a 

W 



/ cose \ 

' a 



cos6,_ 



\ cos9 o/ 
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Givarens vridningsvinkel a runt denna normal till ytan erhalles till slut 

direkt ur hagondera av a , a. och a . 

a D c 

Om sa ar erforderligt erhalles darefter robothandens translations- och vrid- 
ningshastighet med hjSlp av dif f erentiering av signalerna X och Z- 

5 Ovan har beskrivits hur givaranordningen enligt uppfinningen anvands bade 

for lages- och orienteringsbestamning, men givetvis kan utrustningen enbart 
anvandas for lagesbestamning. 

I fig 1 har fo> enkelhets skull en industrirobot med endast fyra axlar 
visats, men givetvis Sr uppfinningen lika tillSmplig Sven vid robotar med 
10 . ett annat antal frihetsgrader . Uppfinningen ger vidare samma fcJrdelar aven 
vid andra arbetsorgan Sn det i fig 1 visade gripdonet, t ex svetsutrust- 
ningar, bearbetningsverktyg, montageverktyg etc. 

Ovan har beskrivits hur vid den visade tvadimensionella analoga f otodetektorn 
ljuskallorna lampligen pulsas sa att endast en at gangen ar lysande . Vid en 
15 diodarray (CCD-array) Sr en dylik pulsning ej nodvandig, eftersom styrutrust- 
ningen enkelt kan halla reda pa vilken av alia ljusflSckar pa detektorn som 
tillhor respektive ljuskalla, detta speciellt som roboten typiskt sett ror 
sig relativt langsamt. 

Civaren enligt uppfinningen kan utformas p5 ett stort antal andra satt an det 
20 i fig 2 visade. Exempelvis kan den besta av en samling detektorrutor med mot- 
svarande optik, exempelvis sex stycken rutor av vilka var och en tScker 1/6 
av rymden (anordnade som sidorna i en kub). 

Sjalvfallet ar det sa att 1 juskallornas lagen enbart behSver vara kanda 
relativt den arbetspunkt dlt man dnskar att roboten skall rora sig. Om 

25 arbetspunkten for roboten befinner sig pa ett rorligt foreraal satts dar- 

Tor med forde] ljuskallorna fast pa detta , varigenom automatisk foljning 
av foremalet erhalles. I detta fall torde det dock ofta vara larapligt att 
aven ha ett antal fasta ljuskallor sa att kontroll hela tiden erhalles pa 
robotens lage relativt ett fast koordinatsystem (sa att systemet inte dri- 

30 ver ivag). Vaxlingen mellan dessa tva system kan ske kontinuerligt , och 

styrsystemet kan exempelvis samtidigt halla reda pa robotens lage i bagge 
systeraen men enbart styra robotens rorelse med hjalp av det ena systemet, 
foretradesvis da det som ger den storsta noggrannheten (om detta Mr mdjligt) . 
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Ljuskallorna bor enligt kSnda geometriska principer fo> att erna hogsta moj- 
liga noggrannhet placeras sa att, na*r roboten befinner sig i sitt onskade 
arbetsomrade , syftlinjerna mellan givaren och de anvanda ljuskallorna bildar 
vinklar med varandra som inte ar alltfor sma. 

5 Uppfinningen har ovan beskrivits i anslutning till en utrustning som arbetar 
med pptisk straining. Det a> dSrvid sjSlvklart att den anvanda stralningen 
kan ligga sava'l inom som utanfdr det synliga vaglSngdsbandet . En utrustning 
enligt uppfinningen kan emellertid anordnas att arbeta aven med andra signa- 
ler an ljus. Ett exempel pa en sadan signalform a> ultral jud. I detla fall 

10 ersatts ljuskallorna i den ovan beskrivna anordningen med ultral judgeneratorer , 
och givarna 30 etc med riktningskSnnande ultral judmottagare . BerakningsgSng 
och fSrdelar blir 1 detta fall desamma. 

Som framgar av ovanstaende beskrivning erbjuder en industrirobot enligt upp- 
finningen stora fordelar jamfort med hittills kanda och anvfinda robotar. 

15 Genom att robothandens lage direkt bestams av givaren, som Sr anordnad vid 

eller pa handen, erhalles en hog noggrannhet hos lagesbestSmningen oberoende 
av tillverknings- och monteringstoleranser hos robotens delar och oberoende 
av fel i signalerna Tran de i robotens olika axlar anordnade vinkelgivarna . 
Likasa bibehalles den hoga noggrannheten i lSgesbestSmningen oberoende av de 

20 deformationer som vid hog belastning eller h6ga accelerationer kan upptrada 
i roboten. Dessa fakta gor det roojligt att astadkomma en robot som trots be- 
tydligt ]agre toleranskrav och styvhetskrav uppvisar en mycket hog noggrann- 
het i lagesbestamningen. 

En ytterligare vasentlig fdrdel hos uppfinningen Sr att ett robotexemplar 
25 direkt kan utbytas mot ett annat, t ex vid drif tavbrott , utan att nagon som 
heist omprograramering erfordras. 

Genoro att de slgnalutsSndande organen (ljuskallorna) IMmpligen kan anbringas 
fast f orbundna • med ett arbetsstycke och nara den aktuella arbetspunkten for 
roboten erhalles den viktiga fordelen, att roboten autoreatiskt kommer att 

30 folja rCrelser eller lagesavvikelser hos arbetsstycket . Vidare erhalles genom 
att placera de signalutsandande organen nara arbetspunkten en mycket hog nog- 
grannhet i nSrheten av arbetspunkten, dar detta Sr synnerligen onskvSrt, 
medan ISngre fran arbetspunkten en ISgre noggrannhet erhalles, vilket kan 
accepteras utan olSgenheter. Cenom att enligt uppfinningen lagesbestamningen 

35 erhalles med onskad hog noggrannhet utan att robotens fastsattningspunkt 
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behover vara kand kan roboten i princip tillatas rSra sig fritt i arbets- 
lokalen, varigenom dess arbetsorarade kan utstrackas hbgst vSsentligt JSm- 
fort med tidigare kanda fasta eller begransat akbara. robotar . 

Enligt en ytterligare variant av uppfinningen kan de ovan beskrivna signal - 
5 kallorna (ljuskallorna) vara passiva (aterutsandande eller ref lekterande) 

organ, varvid en signalutsSndande anordning placeras pa annan plats, t ex 
vid givaren. 

Flera andra satt finns att anvanda en industrirobot enligt uppfinningen. 
Ett sadant sStt a> att anvanda givaren enligt uppfinningen enbart som 

10 la*gesgivare i reglsrloopen och att anvSnda konventioneila hastighets- 

givare, t ex tachometrar i varje robotaxel , som hastighetsaterfdring ror 
reglersysteroet . Ett annat alternativ ar att anvanda givaren som komplement 
till konventioneila lagesgivare for kalibrering eller kontroll, for att 
oka noggrannheten vid lSgesbestamningen , eller fdr inmjitning fdr att moj- 

15 liggora utbyte av robotar. Ett ytterligare alternativ ar att enbart anvanda 
givaren sotn hastighetsgivare . 

Vid utbyte av en industrirobot mot ett annat exemplar, vilket skall utfora 
samma funktion pa sarama stalle, ar det inte nddvSndigt att signalk^llornas 
lSgen ar kanda. I detta fall ar det namligen endast nedvSndigt for det nya 
20 robotexemplaret att kunna aterinta de positioner, vid vilka ISgena hos 
signalkSllornas avbildningar pa detektorn overenssta*mmer med de som er- 
h611s vid den ursprungliga roboten i motsvarande positioner. 

PATENTKRAV 

1. Industrirobot (2) med ett flertal relativt varandra rorliga delar 
(21-25) och forsedd med en lagesgivaranordning (30, 52) som innef attar en 
med en av robotens rorliga delar (25) mekaniskt forbunden givare (30). 
Givaren (30) Sr anordnad att mottaga signaler fran en uppsattning (111-44) 
av minst tre skilda signalkSllor t vilkas lagen relativt varandra Sr ka*nda. 
Varje signalkSlla ar anordnad att utsanda signaler som fortplantar sig rSt- 
linjigt aver strSckan mellan signalkSllan och givaren ('30). 
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Roboten kannetecknas av att givaren (30) innef attar organ (302, 209-312) 
anordnade att avge signaler (x, y) som definierar riktningen (6, 9) rela- 
tivt givaren hos syftlinjerna fran givaren till var och en av signalk&l- 
lorna. Givaren innef attar vidare berSkningsorgan (52) anordnade att till- 
foras nSmnda riktningsdef inierande signaler (x, y), att med ledning av 
dessa signaler bestSrama vinklarna mellan syftlinjerna fran givaren till 
signalkSllorna och att utgaende fran nSmnda vinklar alstra information 
(X) sora definierar givarens lage relativt signalkallorna. 

2. Industrirobot enligt patentkrav 1, kannetecknad darav, 

att ber&kningsorganen (52) a'r anordnade att utgaende fran namnda vinklar 

alstra information (n , n. , n , a) , som definierar givarens orientering 
a' b* c 0 0 

relativt signalkallorna. 

3* Industrirobot enligt nagot av fdregaende patentkrav, kanne- 
tecknad darav, att signalkSllorna utgSrs av kallor for elektromagne 
tisk straining. 

4. Industrirobot enligt nSgot av foregaende patentkrav, k a' n n e - 

t e c k n a d darav, att lagesgivarutrustningen Sr forsedd med organ for 
periodisk aktivering av signalkallorna i tur och ordning sa att endast en 
av signalkSllorna at gangen £r aktiverad. 

5. Industrirobot enligt patentkrav 3, kSnnetecknad darav, 
att signalkallorna utgors av ljuskSllor och att givaren (30) innefattar 
ett linssystero (300) av vidvinkeltyp t anordnat att avbilda 1 juskSllorna 

pa ett detektororgan (302), samt organ (309-312) anordnade att avge infor- 
mation (x, y) som definierar laget pa detektororganet av en ljuskSllas av- 
bildning, vilken information a> anordnad att tillforas berSkningsorganen 
(52), 

6: Industrirobot enligt nigot av patentkraven 3-5, kanneteck- 
nad darav, att detektororganet utg6rs av en tvadimensionell detektor 
(302). 



7. Industrirobot enligt nagot av patentkraven 3-5, kanneteck- 
nad darav, att detektororganet utg6rs av tva endimensionella detektorer 
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